
#include "MotionRequestServer.h"

#include <stdio.h>
#include <stdlib.h>
#include <iostream>
#include "MotionTaskQueue.h"
#include "rapidjson/document.h"
#include "rapidjson/writer.h"
#include "rapidjson/stringbuffer.h"

#include <boost/shared_ptr.hpp>
#include <boost/make_shared.hpp>
#include <boost/function.hpp>
#include <boost/bind.hpp>
#include <boost/asio/placeholders.hpp>
#include <boost/asio/read.hpp>
#include <boost/asio/write.hpp>
#include <boost/array.hpp>
#include <vector>


MotionRequestServer::MotionRequestServer(boost::asio::io_service &io_svc)
    : m_io_svc(io_svc)
    , m_acceptor(m_io_svc)
    , tagLenM(10)
    , isRun(false)
	, isWaitCon(true)
{
	io_context = new boost::asio::io_context;
    new_sock = new boost::asio::ip::tcp::socket(*io_context);
}

MotionRequestServer::~MotionRequestServer()
{
	if(new_sock)
	{
		new_sock->close();
		delete new_sock;
	}
	if(io_context)
	{
		delete io_context;
	}
}

bool MotionRequestServer::listen(const char *lis_ip, const char *lis_port)
{
    if (lis_port == NULL || lis_port[0] == 0)
    {
        return false;
    }

    unsigned short us_port = 0;
    if (sscanf(lis_port, "%hu", &us_port) != 1)
    {
        return false;
    }

    return listen(lis_ip, us_port);
}

bool MotionRequestServer::listen(const char *lis_ip, unsigned short lis_port)
{
    boost::asio::ip::address_v4 lis_addr;
    if (lis_ip != NULL && lis_ip[0] != 0)
        lis_addr = boost::asio::ip::address_v4::from_string(lis_ip);

    boost::asio::ip::tcp::endpoint lis_ep;
    lis_ep.address(lis_addr);
    lis_ep.port(lis_port);

    boost::system::error_code ec_temp;
    m_acceptor.open(boost::asio::ip::tcp::v4());
    m_acceptor.bind(lis_ep, ec_temp);
    if (ec_temp)
    {
        return false;
    }
    m_acceptor.listen();
    isRun = true;

    return true;
}

bool MotionRequestServer::listen(const char *lis_port)
{
    unsigned short us_port = 0;
    sscanf(lis_port, "%hu", &us_port);

    return listen("", us_port);
}

bool MotionRequestServer::listen(unsigned short lis_port)
{
    return listen("", lis_port);
}

void MotionRequestServer::run()
{
    while (isRun)
    {
        try
        {
            while(isWaitCon)
            {
				m_acceptor.accept(*new_sock);
				isWaitCon = false;
				//break;
			}

            // 接收报文头，第1-10位是报文长度，第11开始是报文的json string 
            char jsonLen[32];
            memset(jsonLen, 0, sizeof(jsonLen));
            boost::system::error_code ec;
            int recvTagLen = 0;
            size_t bytes_transferred = boost::asio::read(*new_sock, boost::asio::buffer(jsonLen, tagLenM)); // 读取21个字节
            if (bytes_transferred != tagLenM)
            {
                std::cout << "Recv Tag Len Error:<" << bytes_transferred << ">" << std::endl;
                continue;
            }

            int jLen = atoi(jsonLen);

            char* jBuf = new char[jLen];
            size_t bytesRead = boost::asio::read(*new_sock, boost::asio::buffer(jBuf, jLen));
            if (bytesRead != jLen)
            {
                std::cout << "Recv Json Len Error:<" << bytesRead << ">" << std::endl;
                continue;
            }

            rapidjson::Document document;
            document.Parse(jBuf);
			if (document.HasParseError()) 
			{
                // 解析失败处理
				printf("Parse Json data error:--->\n");
				printf(jBuf);
				printf("<-----\n");
                continue;
            }
			
			if (document.HasMember("CMD") && document["CMD"].IsString()) 
			{
                std::string cmdStr = document["CMD"].GetString();
				if(cmdStr == "GetData")
				{
					printf("Recv GetData CMD...\n");
					
					Data_Request dataReq;
					const rapidjson::Value& DataName = document["DataName"];
					for (rapidjson::SizeType i = 0; i < DataName.Size(); i++)
					{
						dataReq.DataNameList.push_back(DataName[i].GetString());
						
						printf("Recv GetData <%s>...\n", DataName[i].GetString());
					}
					dataReq.reqSocket = new_sock;
					
					MotionTaskQueue::instance()->mtxReqData.lock();
					MotionTaskQueue::instance()->reqData.push(dataReq);
					MotionTaskQueue::instance()->mtxReqData.unlock();
				}
				else if(cmdStr == "MoveToPos")
				{
					printf("Recv MoveToPos CMD...\n");
					
					Motion_Request motionReq;
					motionReq.CmdStr = cmdStr;
					motionReq.targetX = atof(document["PosX"].GetString());
					motionReq.targetY = atof(document["PosY"].GetString());
					motionReq.targetZ = atof(document["PosZ"].GetString());
					motionReq.targetR = atof(document["PosR"].GetString());
					motionReq.moveSpeed = atoi(document["Speed"].GetString());
					motionReq.reqSocket = new_sock;
					MotionTaskQueue::instance()->reqMotion.push(motionReq);
				}
				else if(cmdStr == "AllZero")
				{
					printf("Recv AllZero CMD...\n");
					
					Motion_Request motionReq;
					motionReq.CmdStr = cmdStr;
					motionReq.moveSpeed = atoi(document["Speed"].GetString());
					motionReq.reqSocket = new_sock;
					MotionTaskQueue::instance()->reqMotion.push(motionReq);
				}
				else
				{
					printf("Recv UnKnow CMD...\n");
				}
            } 
			else 
			{
                printf("Not Find Cmd Data...\n");
            }
			
            delete jBuf;
        }
        catch (std::exception& e)
        {
			new_sock->close();
			isWaitCon = true;
            std::cerr << "Exception: " << e.what() << std::endl;
        }
    }
}

void MotionRequestServer::stop()
{
    isRun = false;
    m_acceptor.close();
}

